#include "trackingsystem.h"

#include "ARToolKitPlus/TrackerMultiMarkerImpl.h"

#include "logging.h"


std::string TrackingSystem::sConfigFilename      = "ar_config.cfg";
std::string TrackingSystem::sCalibrationFilename = "ar_calib.cal";
bool TrackingSystem::sIsUsingFullResImage        = true;
bool TrackingSystem::sIsUsingHistory             = true;
bool TrackingSystem::sIsUsingAutoThreshold       = true;
int TrackingSystem::sThreshold                   = 140;

TrackingSystem::TrackingSystem()
{
	mInitialized = false;
	mMarkersFound = false;

	mTracker = 0;
	mNumDetected = 0;
}

TrackingSystem::~TrackingSystem()
{
	delete mTracker;
}

void TrackingSystem::Init(int width, int height)
{
	mTracker = new ARToolKitPlus::TrackerMultiMarkerImpl<6, 6, 6, 2, 8>(width, height);


	mTracker->setPixelFormat(ARToolKitPlus::PIXEL_FORMAT_BGR); //PIXEL_FORMAT_LUM


	if(!mTracker->init(TrackingSystem::sCalibrationFilename.c_str(), TrackingSystem::sConfigFilename.c_str(), 5.0f, 50000.0f))
	{
		Logging::getSingleton().Output("MultiTracker init failed : calibration file not found");
	}

	//Set Marker border size : thin = 0.125f & large = 0.250f
	mTracker->setBorderWidth(0.125f);

	mTracker->setUndistortionMode(ARToolKitPlus::UNDIST_LUT);
	mTracker->setMarkerMode(ARToolKitPlus::MARKER_ID_BCH);
	mTracker->changeCameraSize(width, height);

	//Set image full res analysis on or off
	if (TrackingSystem::sIsUsingFullResImage)
		mTracker->setImageProcessingMode(ARToolKitPlus::IMAGE_FULL_RES);

	//Set History on or off
	mTracker->setUseDetectLite(!TrackingSystem::sIsUsingHistory);

	//Set Threshold value or use auto-thresholding
	mTracker->activateAutoThreshold(TrackingSystem::sIsUsingAutoThreshold);	
	if (!TrackingSystem::sIsUsingAutoThreshold)
		mTracker->setThreshold(TrackingSystem::sThreshold);

	mInitialized = true;
}

int TrackingSystem::Update(const unsigned char* data)
{
	if (!mInitialized)
		return 0;

	//calc() method return the number of markers found
	mNumDetected = mTracker->calc(data);

	if (mNumDetected > 0)
	{		
		mMarkersFound = true;
	}
	else
	{
		mMarkersFound = false;
	}

	return mNumDetected;
}



float* TrackingSystem::Convert(const ARFloat _trans[3][4]) const
{
	float m[16];
	for (int i = 0; i<3; i++)
		for (int j=0; j<4; j++)        
			m[i * 4 + j] = _trans[i][j];
	m[12] = m[13] = m[14] = 0.0;
	m[15] = 1.0;

	return m;
}

const std::vector<int> TrackingSystem::GetVisibleMarkersId() const
{
	std::vector<int> ids;

	int* markersIds;
	mTracker->getDetectedMarkers(markersIds);
	for (int i=0; i<mTracker->getNumDetectedMarkers(); ++i)
		ids.push_back(markersIds[i]);

	return ids;
}

const ARToolKitPlus::ARMarkerInfo& TrackingSystem::GetDetectedMarker(int nWhich) const  
{
	return mTracker->getDetectedMarker(nWhich);  
}

float TrackingSystem::GetMarkerDepth(ARToolKitPlus::ARMarkerInfo* markerInfo,TrackerObject* obj)
{
	mTracker->arGetTransMat(markerInfo, obj->mMarkerCenter, obj->mMarkerWidth, obj->mTrans);
	return obj->mTrans[2][3];

}

float TrackingSystem::GetMarkerDepthCont(ARToolKitPlus::ARMarkerInfo* markerInfo, TrackerObject* obj)
{
	mTracker->arGetTransMatCont(markerInfo, obj->mTrans, obj->mMarkerCenter, obj->mMarkerWidth, obj->mTrans);
	return obj->mTrans[2][3];

}